#include "navmesh.h"
extern "C"
{
#include "c_navmesh.h"
}

struct CNavMesh : NavMesh
{
	CNavMesh() {}
	~CNavMesh() {}
};

CNavMesh *NewCNavMesh()
{
	auto p = new CNavMesh();
	return p;
}

void DeleteMCNavMesh(CNavMesh *p)
{
	delete p;
}

bool Load(CNavMesh *p, const char *path)
{
	return p->LoadSoloNavMesh(path);
}

bool FindPath(CNavMesh *p, float spos[3], float epos[3], float *result, int *size)
{
	p->FindPath(spos, epos);
	int index = 0;
	for (auto it : p->m_pathIndex) 
	{
		float x = p->m_straightPath[(it)*3];
		float z = p->m_straightPath[(it)*3 + 1];
		float y = p->m_straightPath[(it)*3 + 2];
		result[index * 3] = x;
		result[index * 3 + 1] = z;
		result[index * 3 + 2] = y;
		index++;
	}
	*size = index;

	// memcpy(result, p->m_straightPath, p->m_nstraightPath * 3 * sizeof(float));
	// *size = p->m_nstraightPath;
	// for (int i = 0; i < p->m_nstraightPath; i++)
	// {
	// 	std::cout << p->m_straightPath[i * 3] << " " << p->m_straightPath[i * 3 + 2] << std::endl;
	// }

	return true;
}

bool RandomFindPath(CNavMesh *p, float *result, int *size)
{
	float spos[3];
	float epos[3];

	dtPolyRef start_ref;
	dtPolyRef end_ref;

	dtStatus status = p->m_navQuery->findRandomPoint(&p->m_filter, frand, &start_ref, spos);
	if (dtStatusSucceed(status))
	{
		status = p->m_navQuery->findRandomPoint(&p->m_filter, frand, &end_ref, epos);
		if (dtStatusSucceed(status))
		{
			// std::cout << "spos:" << spos[0] << " " << spos[1] << " " << spos[2] << std::endl;
			// std::cout << "epos:" << epos[0] << " " << epos[1] << " " << epos[2] << std::endl;
			p->FindPath(spos, epos);
			// for (int i = 0; i < p->m_nstraightPath * 3 && i < MAX_POLYS * 3; i++)
			// {
			// 	result[i] = p->m_straightPath[i];
			// }
			int index = 0;
			for (auto it : p->m_pathIndex) 
			{
				float x = p->m_straightPath[(it)*3];
				float z = p->m_straightPath[(it)*3 + 1];
				float y = p->m_straightPath[(it)*3 + 2];
				result[index * 3] = x;
				result[index * 3 + 1] = z;
				result[index * 3 + 2] = y;
				index++;
			}
			*size = index;
			
			// memcpy(result, p->m_straightPath, p->m_nstraightPath * 3 * sizeof(float));
			// *size = p->m_nstraightPath;
			return true;
		}
	}
	return false;
}

bool FindRandomPoint(CNavMesh *p,float *start_pos, float *end_pos)
{
	float spos[3];
	float epos[3];

	dtPolyRef start_ref;
	dtPolyRef end_ref;

	dtStatus status = p->m_navQuery->findRandomPoint(&p->m_filter, frand, &start_ref, spos);
	if (dtStatusSucceed(status))
	{
		status = p->m_navQuery->findRandomPoint(&p->m_filter, frand, &end_ref, epos);
		if (dtStatusSucceed(status))
		{
			memcpy(start_pos, spos, 3 * sizeof(float));
			memcpy(end_pos, epos, 3 * sizeof(float));
			return true;
		}
	}
	return false;
}